#include "stm32f10x.h"
#include "I2C_SOFT_GPIO.h"
#include "Mpu_6050.h"
#include "MPU6050_Reg.h"
#include "Delay.h"


const static uint8_t slave_address = 0x68;

void init_mpu_6050(void){

    // 电源管理1
    // 重置（复位）所有寄存器
    write(slave_address,(uint8_t[]){MPU6050_PWR_MGMT_1, 0x80},2);

    // 等待复位完成
    Delay_ms(100);

    // 关闭睡眠（唤醒）、使用陀螺仪的时钟
    write(slave_address,(uint8_t[]){MPU6050_PWR_MGMT_1, 0x01},2);

    write(slave_address,(uint8_t[]){MPU6050_PWR_MGMT_2, 0x00},2);		//电源管理寄存器2，保持默认值0，所有轴均不待机


    // 数字低通滤波器配置为6
    // 一旦配置了 低通滤波器，那么采样率就只能为 1kHz 而不是 8kHz
    write(slave_address,(uint8_t[]){MPU6050_CONFIG,0x06},2);

    
    // 配置采样率分频系数
    // 那么实际的采样率 = 1kHz/(1+4) = 200Hz
    write(slave_address,(uint8_t[]){MPU6050_SMPLRT_DIV,0x04},2);
    
    // 配置加速度量程为+- 2g
    // 并进行加速度计自检
    write(slave_address,(uint8_t[]){MPU6050_ACCEL_CONFIG,0x00 },2);
    
    // 配置角速度传感器的量程为 +- 250°/s
    // 并进行陀螺仪三轴自检
    write(slave_address,(uint8_t[]){MPU6050_GYRO_CONFIG,0x00},2);
}

int16_t read2Bytes(uint8_t registerAddress){

    
    
    uint8_t temp[2];
    
    // 对于 mpu6050 一次性最多只能读取 24 个字符，即 3 字节的数据
    // 读取时需要指定目标设备的地址 1 字节
    // 读取两个 uint8_t 组合为一个 int16_t  共2字节
    readNByte(slave_address,registerAddress,temp,2);
    
    uint16_t combineNum = 0;
    
  
    combineNum = temp[0];
    combineNum<<=8;
    combineNum|=temp[1];
    
    return combineNum;
}

void read_data(mpu6050_data * dest){

    dest->x_acc = read2Bytes(MPU6050_ACCEL_XOUT_H);
    dest->y_acc = read2Bytes(MPU6050_ACCEL_YOUT_H);
    dest->z_acc = read2Bytes(MPU6050_ACCEL_ZOUT_H);

    
    dest->x_gyr = read2Bytes(MPU6050_GYRO_XOUT_H);
    dest->y_gyr = read2Bytes(MPU6050_GYRO_YOUT_H);
    dest->z_gyr = read2Bytes(MPU6050_GYRO_ZOUT_H);

   
    
}
